/*
 * WinkelBerechnung.c
 *
 *  Created on: 14.06.2009
 *      Author: michi
 */
//#include "vorwaertskinematik.h"


#include "../vars.h"


void aktualisiereMatrixFuerTheta4() {

	//	setzeAktuelleGleichung((ermittelTheta_4Wert));
	//	sekantenVerfahren();


	DrehM11s.m11 = cos(theta_1);
	DrehM11s.m12 = -sin(theta_1);
	DrehM11s.m21 = sin(theta_1);
	DrehM11s.m22 = cos(theta_1);

	r30 = vektorAddition(r10, (multipliziereMitVektor(DrehM01,
			multipliziereMitVektor(DrehM11s, s1r31))));
	// r60= vektorAddition(r40,( multipliziereMitVektor(DrehM04, multipliziereMitVektor(DrehM44s,s4r64))));
	//r30= r10+ DrehM01 * DrehM11s* s1r31;
	//r60= r40+ DrehM04 * DrehM44s* s4r64;

}



double ermittelTheta_4(double t4) {
	double x;

	DrehM44s.m11 = cos(t4);
	DrehM44s.m12 = -sin(t4);
	DrehM44s.m21 = sin(t4);
	DrehM44s.m22 = cos(t4);

	r60 = vektorAddition(r40, (multipliziereMitVektor(DrehM04,
			multipliziereMitVektor(DrehM44s, s4r64))));
	r36 = vektorSubtraktion(r60, r30);
	x = sqrt(punktprodukt(r36, r36));

	return x - 200;
}

double ermittelTheta_2(double t2) {
	//DrehM11s //T11a

	DrehM02t = multipliziere2Matrizen(DrehM01, DrehM11s);//(alpha beta gamma )* theta1


	DrehM22s.m22 = cos(t2);//T22a
	DrehM22s.m23 = -sin(t2);
	DrehM22s.m32 = sin(t2);
	DrehM22s.m33 = cos(t2);

	DrehM02 = multipliziere2Matrizen(DrehM02t, multipliziere2Matrizen(
			EinheitsMatrix, DrehM22s));

	r0ez2a.x = DrehM02.m13;
	r0ez2a.y = DrehM02.m23;
	r0ez2a.z = DrehM02.m33;
	r0ez2a = vektorNormalisieren(r0ez2a);

	return punktprodukt(r0ez2a, r36);

}

double ermittelTheta_5(double t5) {

	DrehM05t = multipliziere2Matrizen(DrehM04, DrehM44s);//(alpha beta gamma )* theta4


	DrehM55s.m22 = cos(t5);//T55a
	DrehM55s.m23 = -sin(t5);
	DrehM55s.m32 = sin(t5);
	DrehM55s.m33 = cos(t5);

	DrehM05 = multipliziere2Matrizen(DrehM05t, multipliziere2Matrizen(
			EinheitsMatrix, DrehM55s));
	r0ez5a.x = DrehM05.m13;
	r0ez5a.y = DrehM05.m23;
	r0ez5a.z = DrehM05.m33;

	return punktprodukt(r0ez5a, r36);

}

double ermittelTheta_3(double t3) {

	DrehM33s.m11 = cos(t3);
	DrehM33s.m12 = -sin(t3);
	DrehM33s.m21 = sin(t3);
	DrehM33s.m22 = cos(t3);
	//printf("iteration t3: %f\n",t3);
	//	printMatrix(DrehM33s);
	DrehM03 = multipliziere2Matrizen(DrehM02, DrehM33s);

	r0ex3a.x = DrehM03.m11;
	r0ex3a.y = DrehM03.m21;
	r0ex3a.z = DrehM03.m31;

	r0ex3a = vektorNormalisieren(r0ex3a);
	double x = punktprodukt(r0ex3a, r36);
	return (x - 200);
}

double ermittelTheta_6(double t6) {
	DrehM66s.m11 = cos(t6);
	DrehM66s.m12 = -sin(t6);
	DrehM66s.m21 = sin(t6);
	DrehM66s.m22 = cos(t6);

	DrehM06 = multipliziere2Matrizen(DrehM05, DrehM66s);

	r0ex6a.x = DrehM06.m11;
	r0ex6a.y = DrehM06.m21;
	r0ex6a.z = DrehM06.m31;

	r0ex6a = vektorNormalisieren(r0ex6a);
	double x = punktprodukt(r0ex6a, r36);

	return (x + 200);
}

double ermittelTheta_7() {

	double x = punktprodukt(r0ez2a ,r0ez5a);
	return acos(x);

}
